#!/home/lin/software/miniconda3/envs/th1.11/bin/python
# -*- coding: utf-8 -*-
import open3d as o3d
import numpy as np
import copy

'''
Open3d使用FLANN构建KDTree以便进行快速最近邻检索。
'''

pcd = o3d.io.read_point_cloud("../data/cloud_bin_0.pcd")
pcd.paint_uniform_color([0.5, 0.5, 0.5])
pcd_tree = o3d.geometry.KDTreeFlann(pcd)

pcd.colors[1500] = [1, 0, 0]

#使用 search_radius_vector_3d 去查询所有的和锚点距离小于给定半径的点，将其涂成绿色
print("Find its neighbors with distance less than 0.2, paint green.")
[k, idx, _] = pcd_tree.search_radius_vector_3d(pcd.points[1500], 0.2)
np.asarray(pcd.colors)[idx[1:], :] = [0, 1, 0]

print("Visualize the point cloud.")
o3d.visualization.draw_geometries([pcd])